/*
 *  displayOpenCV.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2007-12-4.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "displayOpenCV.h"

void printCvArr( const CvArr *ArrTmp)
{
	int i, j;
	int type = ((CvMat*)ArrTmp)->type;
	if( (type == 1111638021) | (type == 1111638016) | (type == 1111638018) | (type == 1111638022) ){
		CvMat* Arr;
		Arr=(CvMat*)ArrTmp;
		for (i = 0; i < Arr->rows; i++)
		{
			printf("\n");
			switch (CV_MAT_DEPTH(Arr->type))
			{
				case CV_32F:
				case CV_64F:
					for (j = 0; j < Arr->cols; j++)
						printf ("%8.3f ", (double)cvGetReal2D(Arr, i, j));
					break;
				case CV_8U:
				case CV_16U:
					for(j = 0; j < Arr->cols; j++)
						printf ("%6d ", (int)cvGetReal2D(Arr, i, j));
					break;
				default:
					break;
			}
		}
	}
	else{
		IplImage* Arr;
		Arr=(IplImage*)ArrTmp;
		for ( int k = 0; k < Arr->nChannels; k++ )
		{
			for ( i = 0; i < Arr->height; i++ )
			{
				printf("\n");
				switch (CV_MAT_DEPTH(Arr->depth))
				{
					case CV_32F:
					case CV_64F:
						for (j = 0; j < Arr->width; j++)
							printf ("%8.3f ", (float)cvGetReal3D(Arr, i, j, k));
						break;
					case CV_8U:
					case CV_16U:
						for(j = 0; j < Arr->width; j++)
							printf ("%6d",(int)cvGetReal3D(Arr, i, j, k));
						break;
					default:
						break;
				}
			}
		}
		printf("\n");
	}

	printf("\n");
	
	
}

void imagesc( const char *imagescWindow, const CvArr *ArrTmp )
{
	// Only works with CvMats right now.
	// This is an attempt at remaking the matlab 'imagesc' function. The only colormap right now is 'jet'.
	IplImage *colorMapJet;
	double min, max;
	int type = ((CvMat*)ArrTmp)->type;

	if( (type == 1111638021) | (type == 1111638016) | (type == 1111638018) | (type == 1111638022) ){
		CvMat *Arr;
//		ArrTmp=(CvMat*)ArrTmp
		Arr=cvCloneMat((CvMat*)ArrTmp);
		
		cvMinMaxLoc(Arr, &min, &max, NULL, NULL, NULL);
		double scale=255/(max-min);
		cvScale( Arr, Arr, scale, 0 ); //Normalizes matrix to 0-255 (grayscale)
		
		colorMapJet=cvCreateImage( cvSize(Arr->cols, Arr->rows), IPL_DEPTH_8U, 3);
		int k;

		for (int i=0; i<Arr->cols; i++){
			for (int j=0; j<Arr->rows; j++){
				k=(int)cvGetReal2D( Arr, j, i );			
				if (k==0){
					cvSet2D( colorMapJet, j, i, cvScalar( 0, 0, 0 ));
				}
				if ( k< 51){
					cvSet2D( colorMapJet, j, i, cvScalar( 200+i, (k)*5, 0 ));
				}
				else if ( k< 101){
					cvSet2D( colorMapJet, j, i, cvScalar( 255-(k-50)*5, 255, 0 ));
				}
				else if ( k< 151){
					cvSet2D( colorMapJet, j, i, cvScalar( 0, 255, (k-100)*5 ) );
				}
				else if ( k< 201){
					cvSet2D( colorMapJet, j, i, cvScalar( 0, 255-(k-150)*5, 255 ));
				}
				else{
					cvSet2D( colorMapJet, j, i, cvScalar( 0, 0, 255-(k-200)*3 ));
				}
			}
		}
	}
	else {
		IplImage *Arr;
		Arr=(IplImage*)ArrTmp;	
		cvMinMaxLoc(Arr, &min, &max, NULL, NULL, NULL);
		double scale=255/(max-min);
		cvScale( Arr, Arr, scale, 0 ); //Normalizes matrix to 0-255 (grayscale)
		
		colorMapJet=cvCreateImage( cvSize(Arr->width, Arr->height), IPL_DEPTH_8U, 3);
		int k;
		for (int i=0; i<Arr->width; i++){
			for (int j=0; j<Arr->height; j++){
				k=(int)cvGetReal2D( Arr, j, i );			
				if (k==0){
					cvSet2D( colorMapJet, j, i, cvScalar( 0, 0, 0 ));
				}
				if ( k< 51){
					cvSet2D( colorMapJet, j, i, cvScalar( 200+i, (k)*5, 0 ));
				}
				else if ( k< 101){
					cvSet2D( colorMapJet, j, i, cvScalar( 255-(k-50)*5, 255, 0 ));
				}
				else if ( k< 151){
					cvSet2D( colorMapJet, j, i, cvScalar( 0, 255, (k-100)*5 ) );
				}
				else if ( k< 201){
					cvSet2D( colorMapJet, j, i, cvScalar( 0, 255-(k-150)*5, 255 ));
				}
				else{
					cvSet2D( colorMapJet, j, i, cvScalar( 0, 0, 255-(k-200)*3 ));
				}
			}
		}
	}
	
	cvShowImage( imagescWindow, colorMapJet );
	cvReleaseImage(&colorMapJet);
	
	return;
}

// Code I found that shows how to tell if a CvArr is a CvMat or an IplImage
/*

 const CvArr* arr;
 arr=(CvArr*)ArrTmp;
 
 printf("====== Array ====== \ntype = %d -> \n", type);
 
 if( type == CV_SPARSE_MAT_MAGIC_VAL ){
 printf("sparse matrix\n");
 }else if( type == CV_MATND_MAGIC_VAL ){
 printf("matrix nd\n");
 }else if( type == CV_MAT_MAGIC_VAL ){
 printf("matrix\n");
 }else{
 printf("IplImage?\n");
 IplImage *img = (IplImage *)arr;
 #define dump(val,comment) printf("  %-20s %-10d %s\n",#val,val,comment)
 dump(img->widthStep,   "size of aligned image row in bytes");
 dump(img->depth,       "IPL_DEPTH_8U==8");
 dump(img->nChannels,   "number of channels");
 dump(img->imageSize,   "image data size in bytes");
 dump(img->dataOrder,   "0:interleaved color channel");
 dump(img->origin,      "0:left-top, 1:left-bottom");
//		dump("align",img->align,"ignore");
#undef dump
}

printf("element type = %d (CV_8UC3==16)\n",cvGetElemType(arr) );
printf("width = %d, height = %d\n",cvGetSize(arr).width,cvGetSize(arr).height);

printf("\n");
*/
